//此驱动为M2006电机驱动 由电控组王川负责  电话：17609531594

#include "device.h"


//m2006设置电流函数
void m2006SetCurrent(P_M2006_CTRL pctrl, CAN_HandleTypeDef *hcan)
{
	static uint8_t m2006_p1data[8];
	static uint32_t m2006_txmailbox1;
	
	//注意此ID和M3508ID冲突  和3508一起用的时候要注意
	static CAN_TxHeaderTypeDef M2006_PACK1_HDR =
  {
			0x200,
      0,
      CAN_ID_STD,
      CAN_RTR_DATA,
      8,
      DISABLE
  };
  int i;
	if(pctrl->pack == M2006_PACK_1_4)
  {
			for(i = 0; i <= 3; i++)    
      {
					m2006_p1data[i * 2]         =   pctrl->setcur[i] >> 8;
          m2006_p1data[i * 2 + 1]     =   pctrl->setcur[i] & 0x00ff;
      }
      HAL_CAN_AddTxMessage(hcan, &M2006_PACK1_HDR, m2006_p1data, &m2006_txmailbox1);
  }
}



void pluck_setcurrent(float current,CAN_HandleTypeDef *hcan)
{

	static uint32_t m2006_txmailbox1;
		static uint8_t m2006_p1data[2];
	//注意此ID和M3508ID冲突  和3508一起用的时候要注意
	static CAN_TxHeaderTypeDef M2006_PACK1_HDR =
  {
			0x200,
      0,
      CAN_ID_STD,
      CAN_RTR_DATA,
      8,
      DISABLE
  };
  
					m2006_p1data[0]         =   (int16_t)current>> 8;
          m2006_p1data[1]          =  ((int16_t)current>> 8)& 0x00ff;
      HAL_CAN_AddTxMessage(hcan, &M2006_PACK1_HDR, m2006_p1data, &m2006_txmailbox1);
  
}

//简介：m2006电机反馈复位
void m2006ResetFeedBack(P_M2006_FDBK pfdbk)
{
    P_M2006_FDBK_UNIT pfirst    = &pfdbk->m1;
    int i;
    for(i = 0; i <= 7; i++)
    {
				pfirst[i].angle        = 0;
				pfirst[i].speed        = 0;
				pfirst[i].fdbktime     = 0;
				pfirst[i].count        = 0;
    }
}

//简介：m2006电机反馈信息整理
uint8_t m2006FeedBack(P_CAN_RX pcanrx, P_M2006_FDBK pfdbk)
{
    uint16_t id = pcanrx->hdr.StdId - 0x200;
    P_M2006_FDBK_UNIT pfirst    = &pfdbk->m1;
    
    pfirst[id - 1].fdbktime     = HAL_GetTick();
	pfirst[id - 1].angle        = (pcanrx->data[0] << 8) + pcanrx->data[1];
    pfirst[id - 1].speed        = (pcanrx->data[2] << 8) + pcanrx->data[3];
    pfirst[id - 1].count        = pfirst[id - 1].count + 1;
		return id;
}


